#include "input.h"
#include "Henning_PID.h"

#ifndef _DRIVETRAIN_H
#define _DRIVETRAIN_H

#define CRAB_ANGLE 45
#define CRAB_THRESHOLD .6
#define V_THRESHOLD 1
#define ANGDIF ((CRAB_THRESHOLD+V_THRESHOLD)/2)
#define DIFF_CONST 2.5

class DriveTrain {
	InputHandler *m_input;
	Jaguar *m_frontRight, *m_frontLeft, *m_backRight, *m_backLeft;
	Victor *m_frontSteer, *m_backSteer;
public:
	DriveTrain::DriveTrain(InputHandler *input);
	void WheelDrive();
	void TestDrive();
	void CrabDrive(float target_angle, float speed);
	void VDrive(float front_angle, float back_angle, float speed);
	void Tank(float left, float right);
	
	void SetSpeed(float speed, float dif=0);
	void SteerFront(float angle);
	void SteerBack(float angle);
	
};

float GetOut(float real, float target);

#endif
